#ifndef PIcontrol_h
#define PIcontrol_h

#include "Arduino.h"

class PIinteg 
{
  public:
    
    PIinteg(void);
    PIinteg(int K_i, int ex2);
    PIinteg(int K_i, int ex2, int init);

    int update(int input, int dt);             // returns Ki/s * input, 
                                               // dt in milliseconds
                                               
    int getValue(void);
    void setGains(int K_a, int ex2);  // Ki = K_i * 2^-ex2
    void setIntegrator(int init);


  protected:
    int k_a, kShift;
    long integrator;
};

class PIVinteg 
{
  public:
    
    PIVinteg(void);
    PIVinteg(int K_i, int ex2);
    PIVinteg(int K_i, int ex2, int init[]);
    
    void update(int input[], int output[], int dt);           // returns Ki/s * input, 
                                               // dt in milliseconds
    void setGains(int K_a, int ex2);           // Ki = K_i * 2^-ex2
    void getValue(int output[]);
    void setIntegrator(int init[]);


  protected:
    int k_a, kShift;
    long integrator[3];
};


class LongVinteg : public PIVinteg
{
  public:
    
    LongVinteg(void);
    LongVinteg(int K_i, int ex2);
    LongVinteg(int K_i, int ex2, int init[]);
    
    void update(int input[], int output[], int dt);    // returns Ki/s * input, 
                                                       // dt in milliseconds
                                                       // Ki = K_i * 2^-ex2
    void setGains(int K_a, int ex2);
    void setIntegrator(int init[]);
    

  protected:
    long integrator_low[3];
};


class PIcontrol : public PIinteg
{
  public:
    
    PIcontrol(void);
    PIcontrol(int K_p, int K_a, int ex2);
    PIcontrol(int K_p, int K_a, int ex2, int init);

    int update(int input, int dt);             // returns Ka (1/s + Kp)input, 
                                               // dt in milliseconds
    void setGains(int K_p, int K_a, int ex2);  // K_p = 1000 * Kp, Ka = K_a * 2^-ex2

  protected:
    int k_p;
    long pFrac, fracMask;
};

class PIVcontrol : public PIVinteg
{
  public:
    
    PIVcontrol(void);
    PIVcontrol(int K_p, int K_a, int ex2);
    PIVcontrol(int K_p, int K_a, int ex2, int init[]);

    void update(int input[], int output[], int dt);             // returns Ka (1/s + Kp)input, 
                                               // dt in milliseconds
    void setGains(int K_p, int K_a, int ex2);  // K_p = 1000 * Kp, Ka = K_a * 2^-ex2


  protected:
    int k_p;
    long pFrac[3], fracMask;
};

class PIintegLimit : public PIinteg
{
  public:
    
    PIintegLimit(void);
    PIintegLimit(int K_i, int ex2);
    PIintegLimit(int K_i, int ex2, int limit);

    int update(int input, int dt);             // returns Ki/s * input, 
                                               // dt in milliseconds
    void addInt(long x);
    void setGains(int K_a, int ex2);
    void setGains(int K_a, int ex2, int limit);  // Ki = K_i * 2^-ex2
    void setLimit(int limit);
    void setLimit(int minLim, int maxLim);
    
  protected:
    long minLimit, maxLimit;
};

class PIVintegLimit : public PIVinteg
{
  public:
    
    PIVintegLimit(void);
    PIVintegLimit(int K_i, int ex2);
    PIVintegLimit(int K_i, int ex2, int limit);
    
    void update(int input[], int output[], int dt);           // returns Ki/s * input, 
                                               // dt in milliseconds
    void addInt(long x, int idx);
    void setGains(int K_a, int ex2);           // Ki = K_i * 2^-ex2
    void setGains(int K_a, int ex2, int limit);  // Ki = K_i * 2^-ex2
    void setLimit(int limit);
    void setLimit(int minLim, int maxLim);


  protected:
    long minLimit, maxLimit;
};


class LongVintegLimit : public PIVintegLimit
{
  public:
    
    LongVintegLimit(void);
    LongVintegLimit(int K_i, int ex2);
    LongVintegLimit(int K_i, int ex2, int init[]);
    
    void update(int input[], int output[], int dt);
    
  protected:
    long integrator_low[3];
};


class PIcontrolLimit : public PIintegLimit
{
  public:
    
    PIcontrolLimit(void);
    PIcontrolLimit(int K_p, int K_a, int ex2);
    PIcontrolLimit(int K_p, int K_a, int ex2, int init);

    int update(int input, int dt);             // returns Ka (1/s + Kp)input, 
                                               // dt in milliseconds
    void setGains(int K_p, int K_a, int ex2);  // K_p = 1000 * Kp, Ka = K_a * 2^-ex2
    void setGains(int K_p, int K_a, int ex2, int limit);

  protected:
    int k_p;
    long pFrac, fracMask;
};

class PIVcontrolLimit : public PIVintegLimit
{
  public:
    
    PIVcontrolLimit(void);
    PIVcontrolLimit(int K_p, int K_a, int ex2);
    PIVcontrolLimit(int K_p, int K_a, int ex2, int limit);

    void update(int input[], int output[], int dt);             // returns Ka (1/s + Kp)input, 
                                               // dt in milliseconds
    void setGains(int K_p, int K_a, int ex2);  // K_p = 1000 * Kp, Ka = K_a * 2^-ex2
    void setGains(int K_p, int K_a, int ex2, int limit);


  protected:
    int k_p;
    long pFrac[3], fracMask;
};

class FFFilter : public PIcontrol
{
  public:

    int vOut;

    FFFilter(void);
    FFFilter(int K_p, int K_a, int ex2);

    int update(int input, int feedForward, int dt);
    int update(int input, int dt);
    void setIntegrator2(long init);
    
    
  protected:
    long integ2;
};

class LongFFFilter : public FFFilter
{
  public:
  
    LongFFFilter(void);
    LongFFFilter(int K_p, int K_a, int ex2);
    
    long update(long input, int feedForward, int dt);
    long update(long input, int dt);
  
  protected:
    long outInt;
};

class PIlowGain : public PIcontrol
{
  public:
    
    PIlowGain(void);
    PIlowGain(int K_p, int K_a, int ex2);
    PIlowGain(int K_p, int K_a, int ex2, int init);

    int update(int input, int dt);             // returns Ka (1/s + Kp)input, 
                                               // dt in milliseconds
    void setGains(int K_p, int K_a, int ex2);  // K_p = 10 * Kp, Ka = K_a * 2^-ex2
    void setIntegrator(int init);
   
  protected:

    long integrator_low;  // The low-order 32-bit integrators
};

class PIlowGainLimit : public PIcontrolLimit
{
  public:
    
    PIlowGainLimit(void);
    PIlowGainLimit(int K_p, int K_a, int ex2);
    PIlowGainLimit(int K_p, int K_a, int ex2, int init);

    int update(int input, int dt);             // returns Ka (1/s + Kp)input, 
                                               // dt in milliseconds
    void setGains(int K_p, int K_a, int ex2);  // K_p = 10 * Kp, Ka = K_a * 2^-ex2
    void setGains(int K_p, int K_a, int ex2, int limit);
    void setIntegrator(int init);
   
  protected:

    long integrator_low;  // The low-order 32-bit integrators
};

class PIVlowGain : public PIVcontrol
{
  public:
    
    PIVlowGain(void);
    PIVlowGain(int K_p, int K_a, int ex2);
    PIVlowGain(int K_p, int K_a, int ex2, int init[]);

    void update(int input[], int output[], int dt);             // returns Ka (1/s + Kp)input, 
                                               // dt in milliseconds
    void setGains(int K_p, int K_a, int ex2);  // K_p = 10 * Kp, Ka = K_a * 2^-ex2
    void setIntegrator(int init[]);
    

  protected:
    long integrator_low[3];  // The low-order 32-bit integrators
};

class PIVlowGainLimit : public PIVcontrolLimit
{
  public:
    
    PIVlowGainLimit(void);
    PIVlowGainLimit(int K_p, int K_a, int ex2);
    PIVlowGainLimit(int K_p, int K_a, int ex2, int limit);

    void update(int input[], int output[], int dt);             // returns Ka (1/s + Kp)input, 
    void setGains(int K_p, int K_a, int ex2);  // K_p = 10 * Kp, Ka = K_a * 2^-ex2
    void setGains(int K_p, int K_a, int ex2, int limit);
    void setIntegrator(int init[]);
    

  protected:
    long integrator_low[3];  // The low-order 32-bit integrators
};


#endif